#include "rmgripper.h"
#include "logger.h"
#include "utils.h"

RmGripper::RmGripper()
{

}

RmGripper::~RmGripper()
{
    query_serial.stop();
}

int RmGripper::init()
{
    query_serial.set_params("COM2", \
                            QSerialPort::Baud115200,\
                            QSerialPort::Data8,\
                            QSerialPort::NoParity,\
                            QSerialPort::OneStop,\
                            QSerialPort::NoFlowControl);
    return query_serial.start();
}

int RmGripper::return_to_original()
{
    char cmd[][8]={{0x01,0x05,0x05,0x79,0x00,0x00,0x1c,0xdf},\
                   {0x01,0x05,0x05,0x79,0xff,0x00,0x5d,0x2f}};

    if(0!=cmd_send_and_check(cmd[0],sizeof(cmd[0]))){
        LOG(ERROR)<< "set IO Down Error";
        return -1;
    }
    if(0!=cmd_send_and_check(cmd[1],sizeof(cmd[1]))){
        LOG(ERROR)<< "set IO Up Error";
        return -1;
    }
    return 0;
}

int RmGripper::stop()
{
    //停止命令
    char cmd[][8]={{0x01,0x05,0x05,0x79,0x00,0x00,0x1c,0xdf},\
                   {0x01,0x05,0x05,0x79,0xff,0x00,0x5d,0x2f}};
    if(0!=cmd_send_and_check(cmd[0],sizeof(cmd[0]))){
        LOG(ERROR)<< "set IO down Error";
        return -1;
    }
    if(0!=cmd_send_and_check(cmd[1],sizeof(cmd[1]))){
        LOG(ERROR)<< "set IO Up Error";
        return -1;
    }
    return 0;
}

int RmGripper::trigger_num0_pushing_motion()
{
    LOG(INFO) << "Start of num0 motion";
    //8. 上升沿触发 执行点位0 IO 先写入0 再写入1
    char cmd[][8]={{0x01,0x05,0x05,0x80,0x00,0x00,0xcc,0xee},\
                   {0x01,0x05,0x05,0x80,0xff,0x00,0x8d,0x1e}};
    if(0!=cmd_send_and_check(cmd[0],sizeof(cmd[0]))){
        LOG(ERROR)<< "set IO down Error";
        return -1;
    }
    if(0!=cmd_send_and_check(cmd[1],sizeof(cmd[1]))){
        LOG(ERROR)<< "set IO Up Error";
        return -1;
    }

#if 0
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    //9. 读取 到达点位0 IO 判断指令是否执行完成
    char recv_buf[6]={};
    char check_cmd[8]={0x01,0x01,0x05,0xe2,0x00,0x01,0x5d,0x30};
    if(0 != query_serial.write_buf(check_cmd, sizeof(check_cmd), 500)){
        LOG(ERROR)<< "Send data Error!";
        return -1;
    }
    if(0 >= query_serial.read_buf(recv_buf,sizeof(recv_buf), 500)){
        LOG(ERROR)<< "Receive data Error!";
        return -1;
    }
    short *pRecv_crc = (short *)&recv_buf[4];
    short calc_crc = crc16tablefast(recv_buf,4);
    if(*pRecv_crc!=calc_crc){
        LOG(ERROR)<< "Check crc Error! *pRecv_crc" << *pRecv_crc << "v.s calc_crc:" <<calc_crc;
        return -1;
    }

    char check_bit =recv_buf[3] &0x01;
    if(!check_bit){
        LOG(ERROR)<< "Move is not completed!";
        return -1;
    }
    LOG(INFO) << "End of num0 motion";
#endif

    return 0;
}

//设置流程
int RmGripper::set_num0_pushing_motion()
{
    char cmd[13]={};
    //1. 对 指令表 序号0 指令类型 写入 4（推压运动）
    int fun = INSTRUCTION_FUN_WRITE;
    int addr = INSTRUCTION0_REGISTER_BASE_ADDR;
    char type_data[4]={0x00,0x00,0x00,0x04};
    if(0 != assemble_instruction_cmd(cmd, fun, addr, 2,(char*)type_data, 4)){
        LOG(ERROR) <<"assemble instruction cmd Error!";
        return -1;
    }
#if 0
    std::string log="";
    for (int i = 0; i < 13; i++){
        char tmp[64]={};
        sprintf(tmp, "0x%02x ", cmd[i]&0xff);
        log += tmp;
    }
    LOG(INFO) << "log = " << log;
#endif
    if(0 != send_instruction_cmd(cmd, sizeof(cmd))){
        LOG(ERROR) <<"send instruction cmd Error!";
        return -1;
    }

    //2. 写入速度
    addr = INSTRUCTION0_REGISTER_BASE_ADDR + INSTRUCTION_REGISTER_ADDR_SPEED_OFFSET;
    char speed_data[4]={0x00,0x01,0x38,0x88};
    if(0 != assemble_instruction_cmd(cmd, fun, addr, 2,(char*)speed_data, 4)){
        LOG(ERROR) <<"assemble instruction cmd Error!";
        return -1;
    }
    if(0 != send_instruction_cmd(cmd, sizeof(cmd))){
        LOG(ERROR) <<"send location cmd Error!";
        return -1;
    }

    //3.  加速度
    addr = INSTRUCTION0_REGISTER_BASE_ADDR + INSTRUCTION_REGISTER_ADDR_ACC_OFFSET;
    char ace_data[4]={0x00,0x07,0xa1,0x20};
    if(0 != assemble_instruction_cmd(cmd, fun, addr, 2,(char*)ace_data, 4)){
        LOG(ERROR) <<"assemble instruction cmd Error!";
        return -1;
    }
    if(0 != send_instruction_cmd(cmd, sizeof(cmd))){
        LOG(ERROR) <<"send location cmd Error!";
        return -1;
    }

    //4.  减速度
    addr = INSTRUCTION0_REGISTER_BASE_ADDR + INSTRUCTION_REGISTER_ADDR_DEC_OFFSET;
    char dec_data[4]={0x00,0x07,0xa1,0x20};
    if(0 != assemble_instruction_cmd(cmd, fun, addr, 2,(char*)dec_data, 4)){
        LOG(ERROR) <<"assemble instruction cmd Error!";
        return -1;
    }
    if(0 != send_instruction_cmd(cmd, sizeof(cmd))){
        LOG(ERROR) <<"send location cmd Error!";
        return -1;
    }

    //5. 推压力度
    addr = INSTRUCTION0_REGISTER_BASE_ADDR + INSTRUCTION_REGISTER_ADDR_PUSH_FORCE_OFFSET;
    //0.0014285736083984 = 1%
    char force_data[4]={0x00,0x00,0xBF,0x68};
    if(0 != assemble_instruction_cmd(cmd, fun, addr, 2,(char*)force_data, 4)){
        LOG(ERROR) <<"assemble push force cmd Error!";
        return -1;
    }
    if(0 != send_instruction_cmd(cmd, sizeof(cmd))){
        LOG(ERROR) <<"send push force cmd Error!";
        return -1;
    }

    //6. 推压距离 (距离（单位mm）*1000)
    addr = INSTRUCTION0_REGISTER_BASE_ADDR + INSTRUCTION_REGISTER_ADDR_PUSH_DIST_OFFSET;
    char dist_data[4]={0x00,0x01,0x86,0xa0};
    if(0 != assemble_instruction_cmd(cmd, fun, addr, 2,(char*)dist_data, 4)){
        LOG(ERROR) <<"assemble push dist cmd Error!";
        return -1;
    }
    if(0 != send_instruction_cmd(cmd, sizeof(cmd))){
        LOG(ERROR) <<"send push dist cmd Error!";
        return -1;
    }

    //7. 下一步指令序号
    addr = INSTRUCTION0_REGISTER_BASE_ADDR + INSTRUCTION_REGISTER_ADDR_NEXT_OFFSET;
    char next_data[4]={0xff,0xff,0xff,0xff};
    if(0 != assemble_instruction_cmd(cmd, fun, addr, 2,(char*)next_data, 4)){
        LOG(ERROR) <<"assemble next cmd Error!";
        return -1;
    }
    if(0 != send_instruction_cmd(cmd, sizeof(cmd))){
        LOG(ERROR) <<"send next cmd Error!";
        return -1;
    }

    return 0;
}

int RmGripper::cmd_send_and_check(char *cmd_buf, char buf_size)
{
    char recv_buf[8]={};
    if(0 != query_serial.write_buf(cmd_buf, buf_size, 5000)){
        LOG(ERROR)<< "Send data Error!";
        return -1;
    }
    if(0 >= query_serial.read_buf(recv_buf,sizeof(recv_buf), 5000) ||
            memcmp(cmd_buf, recv_buf, sizeof(recv_buf))){
        LOG(ERROR)<< "Receive data Error!";
        return -1;
    }
    return 0;
}

int RmGripper::assemble_instruction_cmd(char *cmd_buf, char fun, short addr, short num, char *data, char data_size)
{
    if(cmd_buf == nullptr){
        LOG(ERROR)<< "Input cmd buf is nullptr!";
        return -1;
    }
    cmd_buf[0] = 0x01;      //站号
    cmd_buf[1] = fun;       //功能码
    if(INSTRUCTION_FUN_WRITE == fun){
        short tmp = htos(addr);
        memcpy(cmd_buf+2, &tmp, sizeof(short));
        tmp = htos(num);
        memcpy(cmd_buf+4, &tmp, sizeof(short));
        memcpy(cmd_buf+6, &data_size, sizeof(char));
        memcpy(cmd_buf+7, data, data_size);
        short crc =crc16tablefast(cmd_buf, 7+data_size);
        memcpy(cmd_buf+7+data_size, &crc, sizeof(short));
    }else if(INSTRUCTION_FUN_READ == fun){
        short tmp = htos(addr);
        memcpy(cmd_buf+2, &tmp, sizeof(short));
        tmp = htos(num);
        memcpy(cmd_buf+4, &tmp, sizeof(short));
        short crc =crc16tablefast(cmd_buf, 6);
        memcpy(cmd_buf+6, &crc, sizeof(short));
    }else{
        LOG(ERROR) << "fun = " <<fun << "not surport!";
        return -1;
    }
    return 0;
}

int RmGripper::send_instruction_cmd(char *cmd_buf, int buf_size)
{
    char recv_buf[8]={};
    if(0 != query_serial.write_buf(cmd_buf, buf_size, 500)){
        LOG(ERROR)<< "Send data Error!";
        return -1;
    }
    if(0 >= query_serial.read_buf(recv_buf,sizeof(recv_buf), 500)){
        LOG(ERROR)<< "Receive data Error!";
        return -1;
    }

    short* pRecv_crc = (short*)&recv_buf[6];
    short calc_crc = crc16tablefast(recv_buf,6);
    if(calc_crc!= *pRecv_crc){
        LOG(ERROR)<< "Check crc Error! calc_crc:"<< calc_crc << "v.s *pRecv_crc" << *pRecv_crc;
        return -1;
    }

    short *pSend_addr = (short*)&cmd_buf[2];
    short *pRecv_addr = (short*)&recv_buf[2];
    if(*pSend_addr!= *pRecv_addr){
        LOG(ERROR)<< "Check addr Error! *pSend_addr :"<< *pSend_addr << "v.s *pRecv_crc" << *pRecv_crc;
        return -1;
    }

    return 0;
}
